/******************************************************************************
 *
 * Copyright (c) 2019, Intel Corporation
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 *  1. Redistributions of source code must retain the above copyright notice,
 *     this list of conditions and the following disclaimer.
 *
 *  2. Redistributions in binary form must reproduce the above copyright
 *     notice, this list of conditions and the following disclaimer in the
 *     documentation and/or other materials provided with the distribution.
 *
 *  3. Neither the name of the Intel Corporation nor the names of its
 *     contributors may be used to endorse or promote products derived from
 *     this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 ******************************************************************************/

/* This work is licensed under a Creative Commons CCZero 1.0 Universal License.
 * See http://creativecommons.org/publicdomain/zero/1.0/ for more information. */

/**
 * .. _pubsub-tutorial:
 *
 * Working with Publish/Subscribe
 * ------------------------------
 *
 * Work in progress: This Tutorial will be continuously extended during the next
 * PubSub batches. More details about the PubSub extension and corresponding
 * open62541 API are located here: :ref:`pubsub`.
 *
 * Publishing Fields
 * ^^^^^^^^^^^^^^^^^
 * The PubSub publish example demonstrate the simplest way to publish
 * informations from the information model over UDP multicast using the UADP
 * encoding.
 *
 * **Connection handling**
 *
 * PubSubConnections can be created and deleted on runtime. More details about
 * the system preconfiguration and connection can be found in
 * ``tutorial_pubsub_connection.c``.
 */
#define _GNU_SOURCE
#include "open62541.h"
#include <signal.h>
#include <stdio.h>
#include <time.h>
#include <pthread.h>

/* Typically equal to a cycle time. Used to calculate the server starttime for UA_ENABLE_PUBSUB_SOTXTIME */
#define PUBLISHING_INTERVAL_MS 0.125     //milliseconds, decimal cause of backwards compatibility
#define PUBLISHING_INTERVAL_NS (UA_UInt64) (PUBLISHING_INTERVAL_MS * 1000000) //nanoseconds

#ifdef UA_ENABLE_PUBSUB_SOTXTIME
    /* Socket priority is used when mapping to user-defined MQPRIO/TAPRIO qdiscs */
    #define SOCKET_PRIORITY 3

    /* SO_TXTIME-specific additional socket config */
    #define SOTXTIME_DEADLINE_MODE 0
    #define SOTXTIME_RECEIVE_ERRORS 0
    #define SOTXTIME_CLOCKID CLOCK_TAI

    /* TODO: samplingOffset not implemented in open62541 code.
     * OPC UA defines SAMPLING OFFSET as the point of time within a
     * publishingInterval, at which the timer needs to wake up and start
     * collecting data, and calling the respective publish/transmit functions
     * so that packets can go out exactly at publishingOffset, before or at
     * least close to it. This value MUST be less than publishing offset.
     */
    #define SAMPLING_OFFSET 0               //in nanoseconds

    /* In cases where the publishingInterval is very small, SAMPLING_OFFSET
     * might be too short to allow a program to sample the data and send it out
     * at a specific time in the future. EARLY_SAMPLING_OFFSET allows specifying
     * a packet to be transmitted (> publishingInterval) further into the future.
     * It represents the amount of time before a publishingInterval when a
     * sample should be obtained.
     *
     * X = 1st option, point in time where app/ua_server could start sampling data
     * Y = 2nd option, point in time where app/ua_server could start sampling data
     * Z = point in time where a packet should be transmitted.
     *
     *          |<--N x publishingInterval-->|<----publishingInterval---->|
     *    X <-----EARLY_SAMPLING_OFFSET----->|                            |
     *          |                            |<-SAMPLING_OFFSET-> Y       |
     *          |                            |<--PUBLISHING_OFFSET---> Z  |
     *          |                            |                            |
     */
    #define EARLY_SAMPLING_OFFSET 500000         //in nanoseconds

    /* PUBLISHING_OFFSET is the internal target to send out the packets
     * When used with SOTXTIME, packet WILL get transmitted exactly at this
     * point of time. Without SOTXTIME, this is merely the target and
     * transmits roughly at that time.
     */
    #define PUBLISHING_OFFSET 50000   //in nanoseconds

    /* Set the thread's priority for scheduling prioritization and run on
     * a fixed CPU core to avoid context switching.
     */
    #define THREAD_PRIORITY 90
    #define THREAD_CPU_AFFINITY 3
#endif

UA_NodeId connectionIdent, publishedDataSetIdent, writerGroupIdent;

static void
addPubSubConnection(UA_Server *server, UA_String *transportProfile,
                    UA_NetworkAddressUrlDataType *networkAddressUrl){
    /* Details about the connection configuration and handling are located
     * in the pubsub connection tutorial */
    UA_PubSubConnectionConfig connectionConfig;
    memset(&connectionConfig, 0, sizeof(connectionConfig));
    connectionConfig.name = UA_STRING("UADP Connection 1");
    connectionConfig.transportProfileUri = *transportProfile;
    connectionConfig.enabled = UA_TRUE;
    UA_Variant_setScalar(&connectionConfig.address, networkAddressUrl,
                         &UA_TYPES[UA_TYPES_NETWORKADDRESSURLDATATYPE]);
    connectionConfig.publisherId.numeric = UA_UInt32_random();
#ifdef UA_ENABLE_PUBSUB_SOTXTIME
    connectionConfig.sockPrio = SOCKET_PRIORITY;
    connectionConfig.sotxtimeDeadlineMode = SOTXTIME_DEADLINE_MODE;
    connectionConfig.sotxtimeReceiveErrors = SOTXTIME_RECEIVE_ERRORS;
    connectionConfig.sotxtimeClockId = SOTXTIME_CLOCKID;
#endif
    UA_Server_addPubSubConnection(server, &connectionConfig, &connectionIdent);
}

/**
 * **PublishedDataSet handling**
 *
 * The PublishedDataSet (PDS) and PubSubConnection are the toplevel entities and
 * can exist alone. The PDS contains the collection of the published fields. All
 * other PubSub elements are directly or indirectly linked with the PDS or
 * connection. */
static void
addPublishedDataSet(UA_Server *server) {
    /* The PublishedDataSetConfig contains all necessary public
    * informations for the creation of a new PublishedDataSet */
    UA_PublishedDataSetConfig publishedDataSetConfig;
    memset(&publishedDataSetConfig, 0, sizeof(UA_PublishedDataSetConfig));
    publishedDataSetConfig.publishedDataSetType = UA_PUBSUB_DATASET_PUBLISHEDITEMS;
    publishedDataSetConfig.name = UA_STRING("Demo PDS");
    /* Create new PublishedDataSet based on the PublishedDataSetConfig. */
    UA_Server_addPublishedDataSet(server, &publishedDataSetConfig, &publishedDataSetIdent);
}

/**
 * **DataSetField handling**
 *
 * The DataSetField (DSF) is part of the PDS and describes exactly one published
 * field. */
static void
addDataSetField(UA_Server *server) {
    /* Add a field to the previous created PublishedDataSet */
    UA_NodeId dataSetFieldIdent;
    UA_DataSetFieldConfig dataSetFieldConfig;
    memset(&dataSetFieldConfig, 0, sizeof(UA_DataSetFieldConfig));
    dataSetFieldConfig.dataSetFieldType = UA_PUBSUB_DATASETFIELD_VARIABLE;
    dataSetFieldConfig.field.variable.fieldNameAlias = UA_STRING("Server localtime");
    dataSetFieldConfig.field.variable.promotedField = UA_FALSE;
    dataSetFieldConfig.field.variable.publishParameters.publishedVariable =
    UA_NODEID_NUMERIC(0, UA_NS0ID_SERVER_SERVERSTATUS_CURRENTTIME);
    dataSetFieldConfig.field.variable.publishParameters.attributeId = UA_ATTRIBUTEID_VALUE;
    UA_Server_addDataSetField(server, publishedDataSetIdent,
                              &dataSetFieldConfig, &dataSetFieldIdent);
}

/**
 * **WriterGroup handling**
 *
 * The WriterGroup (WG) is part of the connection and contains the primary
 * configuration parameters for the message creation. */
static void
addWriterGroup(UA_Server *server) {
    /* Now we create a new WriterGroupConfig and add the group to the existing
     * PubSubConnection. */
    UA_WriterGroupConfig writerGroupConfig;
    memset(&writerGroupConfig, 0, sizeof(UA_WriterGroupConfig));
    writerGroupConfig.name = UA_STRING("Demo WriterGroup");
    writerGroupConfig.publishingInterval = PUBLISHING_INTERVAL_MS; //ms
    writerGroupConfig.enabled = UA_FALSE;
    writerGroupConfig.writerGroupId = 100;
    writerGroupConfig.encodingMimeType = UA_PUBSUB_ENCODING_UADP;

#ifdef UA_ENABLE_PUBSUB_SOTXTIME
    /* Initialize the first samplingTime, if sampling offset = 0, then it will be the basetime of publishingInterval */
    struct timespec ts_x;
    UA_UInt64 t_sampl_ns;
    /* Get the current time and round of to the nearest 100ns. Then set to the
     * next 100th publishingInterval into the future instead of starting immediately
     */
    clock_gettime(SOTXTIME_CLOCKID, &ts_x);    //TODO change gettime to UA_datetimenow
    t_sampl_ns = (UA_UInt64) ((ts_x.tv_sec * 1000000000) + ((ts_x.tv_nsec/100) * 100));
    t_sampl_ns += (100 * PUBLISHING_INTERVAL_NS) - (t_sampl_ns % PUBLISHING_INTERVAL_NS);
    t_sampl_ns += SAMPLING_OFFSET;

    writerGroupConfig.samplingTime.tv_sec = (UA_Int64) (t_sampl_ns / 1000000000);
    writerGroupConfig.samplingTime.tv_nsec = (UA_Int64) (t_sampl_ns % 1000000000);

    writerGroupConfig.publishingOffset = PUBLISHING_OFFSET;
    writerGroupConfig.earlySamplingOffset = EARLY_SAMPLING_OFFSET;
#endif

    /* The configuration flags for the messages are encapsulated inside the
     * message- and transport settings extension objects. These extension
     * objects are defined by the standard. e.g.
     * UadpWriterGroupMessageDataType */
    UA_Server_addWriterGroup(server, connectionIdent, &writerGroupConfig, &writerGroupIdent);
}

/**
 * **DataSetWriter handling**
 *
 * A DataSetWriter (DSW) is the glue between the WG and the PDS. The DSW is
 * linked to exactly one PDS and contains additional informations for the
 * message generation. */
static void
addDataSetWriter(UA_Server *server) {
    /* We need now a DataSetWriter within the WriterGroup. This means we must
     * create a new DataSetWriterConfig and add call the addWriterGroup function. */
    UA_NodeId dataSetWriterIdent;
    UA_DataSetWriterConfig dataSetWriterConfig;
    memset(&dataSetWriterConfig, 0, sizeof(UA_DataSetWriterConfig));
    dataSetWriterConfig.name = UA_STRING("Demo DataSetWriter");
    dataSetWriterConfig.dataSetWriterId = 62541;
    dataSetWriterConfig.keyFrameCount = 10;
    UA_Server_addDataSetWriter(server, writerGroupIdent, publishedDataSetIdent,
                               &dataSetWriterConfig, &dataSetWriterIdent);
}

/**
 * That's it! You're now publishing the selected fields. Open a packet
 * inspection tool of trust e.g. wireshark and take a look on the outgoing
 * packages. The following graphic figures out the packages created by this
 * tutorial.
 *
 * .. figure:: ua-wireshark-pubsub.png
 *     :figwidth: 100 %
 *     :alt: OPC UA PubSub communication in wireshark
 *
 * The open62541 subscriber API will be released later. If you want to process
 * the the datagrams, take a look on the ua_network_pubsub_networkmessage.c
 * which already contains the decoding code for UADP messages.
 *
 * It follows the main server code, making use of the above definitions. */
UA_Boolean running = true;
static void stopHandler(int sign) {
    UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "received ctrl-c");
    running = false;
}

static int run(UA_String *transportProfile,
               UA_NetworkAddressUrlDataType *networkAddressUrl) {
    signal(SIGINT, stopHandler);
    signal(SIGTERM, stopHandler);

    UA_StatusCode retval = UA_STATUSCODE_GOOD;
    UA_ServerConfig *config = UA_ServerConfig_new_default();
    /* Details about the connection configuration and handling are located in
     * the pubsub connection tutorial */
    config->pubsubTransportLayers =
        (UA_PubSubTransportLayer *) UA_calloc(2, sizeof(UA_PubSubTransportLayer));
    if(!config->pubsubTransportLayers) {
        UA_ServerConfig_delete(config);
        return -1;
    }
    config->pubsubTransportLayers[0] = UA_PubSubTransportLayerUDPMP();
    config->pubsubTransportLayersSize++;
#ifdef UA_ENABLE_PUBSUB_ETH_UADP
    config->pubsubTransportLayers[1] = UA_PubSubTransportLayerEthernet();
    config->pubsubTransportLayersSize++;
#endif
    UA_Server *server = UA_Server_new(config);

    addPubSubConnection(server, transportProfile, networkAddressUrl);
    addPublishedDataSet(server);
    addDataSetField(server);
    addWriterGroup(server);
    addDataSetWriter(server);

    retval |= UA_Server_run(server, &running);
    UA_Server_delete(server);
    UA_ServerConfig_delete(config);
    return (int)retval;
}

#ifdef UA_ENABLE_PUBSUB_SOTXTIME
/* Without setting this, application won't run at high priority, and
 * may get pre-empted by other processes.
 * TODO: change this to multi-arch scalable implementation, possibly a separate commit.
 */
static int set_realtime(pthread_t thread, int priority, uint32_t cpu)
{
	cpu_set_t cpuset;
	struct sched_param sp;
	int err, policy;
	if (priority < 0) {
		fprintf(stderr, "set_realtime() - priority argument < 0\n");
		return -1;
	}
	err = pthread_getschedparam(thread, &policy, &sp);
	if (err) {
		fprintf(stderr, "pthread_getschedparam: %s\n", strerror(err));
		return -1;
	}
	sp.sched_priority = priority;
	err = pthread_setschedparam(thread, SCHED_FIFO, &sp);
	if (err) {
		fprintf(stderr, "pthread_setschedparam: %s\n", strerror(err));
		return -1;
	}
	if (!cpu)
		return 0;
	CPU_ZERO(&cpuset);
	CPU_SET(cpu, &cpuset);
	err = pthread_setaffinity_np(thread, sizeof(cpu_set_t), &cpuset);
	if (err) {
		fprintf(stderr, "pthread_setaffinity_np: %s\n", strerror(err));
		return -1;
	}
	return 0;
}
#endif

static void
usage(char *progname) {
    printf("usage: %s <uri> [device]\n", progname);
}

int main(int argc, char **argv) {
    UA_String transportProfile =
        UA_STRING("http://opcfoundation.org/UA-Profile/Transport/pubsub-udp-uadp");
    UA_NetworkAddressUrlDataType networkAddressUrl =
        {UA_STRING_NULL , UA_STRING("opc.udp://224.0.0.22:4840/")};

#ifdef UA_ENABLE_PUBSUB_SOTXTIME
    if (set_realtime(pthread_self(), THREAD_PRIORITY, THREAD_CPU_AFFINITY)) {
		return -1;
	}
#endif

    if (argc > 1) {
        if (strcmp(argv[1], "-h") == 0) {
            usage(argv[0]);
            return 0;
        } else if (strncmp(argv[1], "opc.udp://", 10) == 0) {
            networkAddressUrl.url = UA_STRING(argv[1]);
        } else if (strncmp(argv[1], "opc.eth://", 10) == 0) {
            transportProfile =
                UA_STRING("http://opcfoundation.org/UA-Profile/Transport/pubsub-eth-uadp");
            if (argc < 3) {
                printf("Error: UADP/ETH needs an interface name\n");
                return 1;
            }
            networkAddressUrl.networkInterface = UA_STRING(argv[2]);
            networkAddressUrl.url = UA_STRING(argv[1]);
        } else {
            printf("Error: unknown URI\n");
            return 1;
        }
    }

    return run(&transportProfile, &networkAddressUrl);
}
